package bojan.robots.motion;

import jade.domain.introspection.GetValue;

import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.List;
import static src.bojan.ConfigurationParams.*;
import bojan.robots.market.Market;

import sim.engine.SimState;
import sim.engine.Steppable;
import sim.field.SparseField;
import sim.util.Double2D;
import src.bojan.Navigator;
import src.bojan.TerrainMap;
import src.bojan.planning.Planner;

public class Navigation implements Steppable {

	private Navigator robot;
	private double maxCost;
	private double cost;
	private Double2D goal;
	private Planner planner;
	private List<Double2D> goals;
	private TerrainMap map;

	public Navigation(Navigator robot, Planner planner, TerrainMap map) {
		super();
		this.robot = robot;
		this.planner = planner;
		goals = planner.initialTour(robot.getPosition());
		this.map = map;
		chooseGoal();
	}

	@Override
	public void step(SimState state) {
		if (cost <= maxCost - 1) {
			// TODO Auto-generated method stub
			robot.move(goal); // TODO The length of steps might be more than 1
			cost++;
			if (robot.getPosition().equals(goal)) {
				goalReached();
			}
		} else {
			goalNotReached();
		}
		if (allCovered()) {
			state.schedule.clear();
		}
	}

	private boolean allCovered() {
		// TODO Parameterize
		return map.visitedInZone(new Double2D(15, 15), 50) > 29 * 29 - 5;
	}

	private void goalNotReached() {
		// TODO Auto-generated method stub
		goalReached();
	}

	private void goalReached() {
		// TODO Make sure a robot doesnt runt out of goals

		offerGoals();
		goals = planner.updateTour(robot.getPosition(), goals);
		chooseGoal();
	}

	public void offerGoals() {
		List<Double2D> tmpGoals = new ArrayList<Double2D>(goals.size());
		for (Double2D double2d : goals) {
			tmpGoals.add(double2d);
		}
		Market market = new Market(NavigationFactory.allAgents());
		for (int i = 0; i < tmpGoals.size(); i++) {
			goal = tmpGoals.get(tmpGoals.size() - 1 - i);
			market.sell(this, goal, value(goal));
		}
	}

	private void chooseGoal() {
		this.goal = goals.remove(0);
		System.out.println(goal);
		cost = 0;
		this.maxCost = planner.estimatedCost(robot.getPosition(), goal);
	}

	public double offer(Double2D goal) {
		if (goals.size() < MAX_NEW_GOALS) {
			return value(goal);
		}
		return 0;
	}

	public double value(Double2D goal) {
		Collection<Double2D> newGoals = new ArrayList<Double2D>();
		newGoals.add(goal);
		List<Double2D> tmp = planner.insertGoals(robot.getPosition(), goals,
				newGoals);
		return planner.value(goal) - cost(tmp, goal);
	}

	public double cost(List<Double2D> potentialGoals, Double2D goal) {
		int ind = potentialGoals.indexOf(goal);
		if (ind == 0) {
			return planner.estimatedCost(robot.getPosition(), goal);
		} else if (ind > 0) {
			return planner.estimatedCost(potentialGoals.get(ind - 1), goal);
		}
		throw new RuntimeException("Goal not inserted.");
	}

	public void setGoals(List<Double2D> goals2) {
		this.goals = goals2;
	}

	public void sold(Double2D double2d) {
		goals.remove(double2d);
	}

	public void bought(Double2D double2d) {
		goals.add(double2d);
		
	}

	public List<Double2D> getGoals() {

		return goals;
	}

}
